#ifndef __TINY_TEST__

#include "vbb_FirmwareUtils.h"
#include "vbb_GSM.h"
#include "vbb_GPS.h"
#include "vbb_SAVE.h"
#include "vbb_Storage.h"
#include "fw/vbb_RTC.h"
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

VBB_USART _futils_usartConsole;
char _futils_consoleBuffer[512];
char _futils_buffer[512];
VBB_Message cmd;


#define MSG_DUMP1(location, len, data) vbb_OUTPUT_PrintNumberUnSigned(location);\
							vbb_OUTPUT_Print("_Length: "); \
							vbb_OUTPUT_PrintNumberUnSigned(len); \
							vbb_OUTPUT_Print("Mesg: ");	\
							vbb_OUTPUT_Print(data); \
							vbb_OUTPUT_Print("\n");
							
#define MSG_DUMP2(location, data) vbb_OUTPUT_PrintNumberUnSigned(location);\
							vbb_OUTPUT_Print("_Mesg: ");	\
							vbb_OUTPUT_Print(data); \
							vbb_OUTPUT_Print("\n");

#define DEBUG_CONSOLE(A) vbb_CONSOLE_Printf(A)

#define PREFIX_CONFIG_CMD1 "CK+"
#define PREFIX_CONFIG_CMD2 "Ck+"
#define PREFIX_CONFIG_CMD3 "cK+"
#define PREFIX_CONFIG_CMD4 "ck+"


void vbb_AdditionalInit(void)
{
    vbb_USART_Init(&_futils_usartConsole, USART1, 9600, 4, _futils_consoleBuffer, 512);    
}

void vbb_CONSOLE_Printf(const char * Format, ...)
{
    va_list args;
	char * buf = _futils_buffer;        
    buf[0] = 0;    
    va_start(args, Format);	
    vsprintf(buf, Format, args);
    vbb_USART_Print(&_futils_usartConsole, _futils_buffer);
    //vbb_OUTPUT_Print(_futils_buffer);
}

char _console_cmd[256];
void vbb_CONSOLE_GotCommand(const char * Command)
{
#ifdef CONFIG_STM32G_MAINTAIN
	memset(_console_cmd, 0x00, sizeof(_console_cmd));
#endif
    vbb_StrCpy(_console_cmd, Command, vbb_GetLen(Command));
    vbb_CleanMessage2(_console_cmd, vbb_GetLen(_console_cmd));
    vbb_CleanMessage(_console_cmd, "\r\n");
    vbb_ProcessCommand(NULL, _console_cmd);
}

void vbb_CONSOLE_Flush(void)
{
#ifdef CONFIG_STM32G_MAINTAIN        
    int newcmd_len;
#endif
    VBB_DAT_Queue * queue = &(_futils_usartConsole.rBuffer);
#ifdef CONFIG_STM32G_MAINTAIN
//    VBB_Message cmd;
#else
    VBB_Message * msg = &(cmd);
#endif
#ifndef CONFIG_STM32G_MAINTAIN	
	vbb_OUTPUT_Print("		vbb_CONSOLE_Flush \n");
	MSG_DUMP1(1, msg->length, msg->buff);
	vbb_OUTPUT_Print("		vbb_CONSOLE_Flush \n");
	MSG_DUMP2(3, _futils_usartConsole.rBuffer.buffer);
    
    while (queue->head < queue->tail)
    {
        c = queue->buffer[queue->head & queue->bufferSizeMinusOne];
        queue->head++;
        
        if (msg->length == 0)
        {
            if (c == 'C')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 1)
        {
            if (c == 'K')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 2)
        {
            if (c == '+')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else
        {
            if (msg->length < MESSAGE_SIZE)
            {
                vbb_Message_AddByte(msg, c);
                if (c == '\r')
                {
                    vbb_Message_AddByte(msg, 0);
                    vbb_CONSOLE_GotCommand(msg->buff);
                    vbb_Message_Init(msg);
                }
            }
            else
            {
                vbb_Message_Init(msg);
            }
        }
    }
#else
    newcmd_len = vbb_DataQueue_Size(queue);
    if(0 == newcmd_len) 
    {
        return;        
    }    

    if(!(vbb_StartWith(queue->buffer, PREFIX_CONFIG_CMD1)||
       vbb_StartWith(queue->buffer, PREFIX_CONFIG_CMD2)||
       vbb_StartWith(queue->buffer, PREFIX_CONFIG_CMD3)||
       vbb_StartWith(queue->buffer, PREFIX_CONFIG_CMD4)))
    {
        vbb_CONSOLE_Printf("%% Unknown command\r\n");
        vbb_DataQueue_memset(queue);
        return;
    }
    
    if(cmd.length == newcmd_len)
    {
        if(0 == memcmp(cmd.buff, queue->buffer, newcmd_len))
        {
            vbb_DataQueue_memset(queue);
            return;                
        }	
    }
    memset(cmd.buff, 0x00, MESSAGE_SIZE);
    memcpy(cmd.buff, queue->buffer, newcmd_len);
    cmd.length = newcmd_len;
    vbb_DataQueue_memset(queue);
    vbb_CONSOLE_GotCommand(cmd.buff);
#endif
}

char * vbb_FindStr(const char * SourceString, const char * MatchString)
{
    return strstr(SourceString, MatchString);
}

char * vbb_StrCpy(char * DestBuffer, const char * SourceBuffer, int Length)
{
    if (Length > 0)
        memcpy(DestBuffer, SourceBuffer, Length);
    DestBuffer[Length] = 0;
    return DestBuffer;
}

unsigned int vbb_GetLen(const char * SrcStr)
{
   return strlen(SrcStr);
}

char * vbb_GetStr(char * DstStr, const char * SrcStr, const char * HeaderStr, const char * FooterStr)
{
    char * BeginStr = vbb_FindStr(SrcStr, HeaderStr);
    char * EndStr;
    if (BeginStr)
    {
        EndStr = vbb_FindStr(BeginStr, FooterStr) + strlen(FooterStr) - 1;    
        if (EndStr)
        {
            DstStr = vbb_StrCpy(DstStr, BeginStr, EndStr - BeginStr + 1);
            return DstStr;
        }
        else
            return NULL;
    }
    else
        return NULL;
}

char * vbb_ExtractStr(char * DstStr, const char * SrcStr, const char * HeaderStr, const char * FooterStr)
{
    char * BeginStr = vbb_FindStr(SrcStr, HeaderStr);
    char * EndStr;
    BeginStr += vbb_GetLen(HeaderStr);
    if (BeginStr)
    {
        EndStr = vbb_FindStr(BeginStr, FooterStr) - 1;    
        if (EndStr)
        {
            DstStr = vbb_StrCpy(DstStr, BeginStr, EndStr - BeginStr + 1);
            return DstStr;
        }
        else
            return NULL;
    }
    else
        return NULL;
}

/**
*   Replace matched string within the message. Watched out the buffer
*/

void vbb_ReplaceStr(char * Message, const char * MatchStr, const char * ReplaceStr)
{
    char * _matchHead;    
    char * _buff;
    char * _msg;
    unsigned int repLen = vbb_GetLen(ReplaceStr);
    unsigned int matLen = vbb_GetLen(MatchStr);
    
    _msg = Message;
    
    _matchHead = vbb_FindStr(_msg, MatchStr);
    
    while (_matchHead)
    {        
        _msg = _matchHead;
        
        _buff = _futils_buffer;
        _buff[0] = 0;        
        vbb_StrCpy(_buff, ReplaceStr, repLen);            
        _buff += repLen;        
        
        vbb_StrCpy(_buff, _msg + matLen, vbb_GetLen(_msg + matLen));
        
        vbb_StrCpy(_msg, _buff, vbb_GetLen(_buff));
        _msg += repLen;
        
        _matchHead = vbb_FindStr(_msg, MatchStr);
    }
}

bool vbb_StartWith(const char * Message, const char * Header)
{
    int len_min = GET_MIN(vbb_GetLen(Message), vbb_GetLen(Header));
    int i;
    for (i = 0; i < len_min; i++)
    {
        if (Message[i] != Header[i])
            return FALSE;
    }
    
    return TRUE;
}

char * vbb_BuildString(const char * Format, ...)
{
    va_list args;
	char * buf = _futils_buffer;        
    buf[0] = 0;    
    va_start(args, Format);	
    vsprintf(buf, Format, args);
    return _futils_buffer;
}

/**
*   Remove unwanted charaters within the message like '\r', '\n'
*/
char _futils_clean[2];
void vbb_CleanMessage(char * Message, const char * UnwantedChars)
{    
    int i = 0;    
    for (i = 0; i < vbb_GetLen(UnwantedChars); i++)
    {
        _futils_clean[0] = UnwantedChars[i];
        _futils_clean[1] = 0;
        vbb_ReplaceStr(Message, _futils_clean, NULL);
    }
}

/*
* Generally, we just need these kind of chars
*/
void vbb_CleanMessage2(char * Message, int Len)
{
    int i = 0;    
    for (i = 0; i < Len; i++)
    {
        if (Message[i] >= 'A'&& Message[i] <= 'Z')
            continue;
        
        if (Message[i] >= 'a' && Message[i] <= 'z')
            continue;
        
        if (Message[i] >= '0' && Message[i] <= '9')
            continue;
        
        if (Message[i] == '(' || Message[i] == ')')
            continue;
        
        if (Message[i] == '[' || Message[i] == ']')
            continue;
        
        if (Message[i] == '%' || Message[i] == ':' || Message[i] == '*' || Message[i] == '/' || Message[i] == '\\')
            continue;
        
        if (Message[i] == '+' || Message[i] == '-' || Message[i] == '^' || Message[i] == '>' || Message[i] == '<')
            continue;
        
        if (Message[i] == '!')
            continue;
        
        if (Message[i] == ',' || Message[i] == '.')
            continue;
        
        if (Message[i] == '\r' || Message[i] == '\n')
            continue;
        
        if (Message[i] == '#')
            continue;
        
        if (Message[i] == '"')
            continue;
        
        Message[i] = ' ';
    }
    Message[Len] = 0;
}

//CK+SMS,"Hello",+84906596015
//CK+SMS,"Hello",0906596015
char _utils_sms_body[256];
char _utils_sms_num[32];
bool Utils_ProcessSMS(const char * MessageBody) 
{
    char * body = _utils_sms_body;
    char * num = _utils_sms_num;
    MessageBody += vbb_GetLen("CK+SMS,\"");
    while (MessageBody[0] != '"')
    {
        body[0] = MessageBody[0];
        MessageBody++;
        body++;        
    }
    body[0] = 0;
    
    MessageBody++;
    MessageBody++;
    while (MessageBody[0] != 0 && MessageBody[0] != '\r' && MessageBody[0] != '\n')
    {
        num[0] = MessageBody[0];
        MessageBody++;
        num++;        
    }
    num[0] = 0;
    
    if (vbb_GSM_SendSMS(_utils_sms_num, _utils_sms_body))
        return TRUE;
    else
        return FALSE;
}

bool Utils_ProcessCmdUpdate(const char * MessageBody)
{
    int version;
    
    MessageBody += vbb_GetLen("CK+UPDATE");
    if (MessageBody[0] == ',')
    {
        sscanf(MessageBody + 1, "%d", &version);
        if (version >= 100 && version <= 999)
        {
            vbb_GSM_StartDownloadFirmware(version, TRUE);
            return TRUE;
        }
        else
            return FALSE;
    }
    else
        return FALSE;
}

extern VBB_Storage storage;
extern VBB_GPS gps;
extern VBB_Sensor sensor;
extern unsigned bool cab_occupied;
extern unsigned int _gsm_LastSendSuccesfully;
extern VBB_FlashMem flashMem;
int hourminsec[3];
int daymonthyear[3];
extern unsigned int server_online;
extern unsigned int LastMeterMessage;

char _utils_statusstr[161];
char * Utils_ProcessStatus(void)
{
    vbb_RTC_Convert(_gsm_LastSendSuccesfully, hourminsec, daymonthyear);
    sprintf(_utils_statusstr, "P %s\nVER %d\nGPS %d.%05dN, %d.%05dE\nVEL %d.%03dKmH\nSENSOR %01d%01d%01d%01d\nQUEUE %u %u\nSERVER %s\nMETER %s\n",
    storage.config.VehicleId,
    VERSION,
    gps.lat_int,
    gps.lat_fra,
    gps.long_int,
    gps.long_fra,
    gps.vel_int,
    gps.vel_fra,
    cab_occupied,
    sensor.EngineStarted,
    sensor.DoorClosed,
    sensor.ACPowered,
    flashMem.sendhead,
    flashMem.tail,    
    (vbb_RTC_GetSec() - server_online) < 1200 ? "ONLINE" : "OFFLINE",
    (vbb_RTC_GetSec() - LastMeterMessage) < 10 ? "ONLINE" : "OFFLINE"
    );
    return _utils_statusstr;
}

extern int _gsm_LastSendData;
extern bool _utils_pulse_simulate;
int _utils_pulse_multiply = 1;
VBB_Pin pin_pulse;

extern unsigned int GSM_State;

bool vbb_ProcessCommand(const char * Number, const char * MessageBody)
{    
    char * status_str;
    int value;
    
    if (vbb_StartWith(MessageBody, "CK+"))
    {          
        if (vbb_StartWith(MessageBody, "CK+DEBUG,"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            value = 0;
            sscanf(MessageBody + vbb_GetLen("CK+DEBUG,"), "%d", &value);
            
            if (value > 0)
                vbb_SetDebugMode(TRUE);
            else
                vbb_SetDebugMode(FALSE);
                            
            if (Number)
            {
                if (vbb_DebugMode())
                    vbb_GSM_SendSMS(Number, "Debug mode enabled.");
                else
                    vbb_GSM_SendSMS(Number, "Debug mode disabled.");
            }
            else
            {
                if (vbb_DebugMode())
                    vbb_OUTPUT_Print("INFO: Debug mode enabled.\r\n");
                else
                    vbb_OUTPUT_Print("INFO: Debug mode disabled.\r\n");
            }
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+STATUS"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            status_str = Utils_ProcessStatus();                        
            if (Number)
                vbb_GSM_SendSMS(Number, status_str);
            else
            {
                vbb_OUTPUT_Print("INFO: Status ");
                vbb_OUTPUT_PrintDebug(status_str);
                vbb_OUTPUT_Print("\r\n");
            }
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+REBOOT"))
        {   
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_OUTPUT_Print("INFO: Reboot.\r\n");
            vbb_Reset();
            if (Number)
                vbb_GSM_SendSMS(Number, "Reboot.");
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+SMS"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            if (Utils_ProcessSMS(MessageBody))
            {
                if (Number)
                    vbb_GSM_SendSMS(Number, "Sent.");
            }
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CANCELUPDATE"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            vbb_GSM_CancelDownloadFirmware();
            if (Number)
                vbb_GSM_SendSMS(Number, "Cancelled update.");            
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+UPDATE"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            if (Utils_ProcessCmdUpdate(MessageBody))
            {   
                if (Number)
                    vbb_GSM_SendSMS(Number, "Started update.");
            }
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+FLUSH"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            _gsm_LastSendData = -(1 << 30);

            if (Number)            
                vbb_GSM_SendSMS(Number, "Flushed.");   
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+FAILSAFE"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_BACKUP_Set(SAVE_GSM_LAUNCH_USERAPP, FALSE);
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Failsafe launched.");
            
            vbb_Reset();            
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+USERAPP"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_BACKUP_Set(SAVE_GSM_LAUNCH_USERAPP, TRUE);
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Userapp launched.");
            
            vbb_Reset();            
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CALL"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            vbb_GSM_MakeMuteCall(MessageBody + vbb_GetLen("CK+CALL,"));
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+BALANCE"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            if (Number)
            {
                status_str = GSM_QueryCUSD("*101#");
                if (status_str)
                    vbb_GSM_SendSMS(Number, status_str);
                else
                    vbb_GSM_SendSMS(Number, "CUSD query failed.");
            }
            else                
                vbb_GSM_QueryCUSD("*101#");            
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CUSD"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");            
            
            if (Number)
            {
                status_str = GSM_QueryCUSD(MessageBody + vbb_GetLen("CK+CUSD,"));
                
                if (status_str)
                    vbb_GSM_SendSMS(Number, status_str);
                else
                    vbb_GSM_SendSMS(Number, "CUSD query failed.");
            }
            else                
                vbb_GSM_QueryCUSD(MessageBody + vbb_GetLen("CK+CUSD,"));
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CONFIG"))
        {            
            vbb_STORAGE_AddConfig(MessageBody);
            vbb_STORAGE_Dump();
            
            if (Number)
                vbb_GSM_SendSMS(Number, MessageBody);
            return TRUE;
        }
#ifdef CONFIG_STM32G_MAINTAIN        
        else if(vbb_StartWith(MessageBody, "CK+SAVECONFIG"))
        {
            vbb_STORAGE_SaveConfig();
            vbb_STORAGE_ReadConfig();
            return TRUE;
        }
#endif        
        else if (vbb_StartWith(MessageBody, "CK+CLEARCONFIG"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_STORAGE_Clear(); 
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Configuration clean.");
            else
                vbb_OUTPUT_Print("INFO: Configuration clean.\r\n");
            vbb_Reset();
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CLEARFLASH"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_FLASHMEM_Clear();
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Flash clean.");
            else
                vbb_OUTPUT_Print("INFO: Flash clean.\r\n");
            
            vbb_Reset();
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+CLEARBACKUP"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_SAVE_Format();
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Backup clean.");
            else
                vbb_OUTPUT_Print("INFO: Backup clean.\r\n");
            
            vbb_Reset();
            return TRUE;
        }
        else if (vbb_StartWith(MessageBody, "CK+FORMAT"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            vbb_SAVE_Format();
            vbb_STORAGE_Clear();
            vbb_FLASHMEM_Clear();            
            
            if (Number)
                vbb_GSM_SendSMS(Number, "Everything clean.");
            else
                vbb_OUTPUT_Print("INFO: Everything clean.\r\n");
            
            vbb_Reset();
            return TRUE;
        }     
        else if (vbb_StartWith(MessageBody, "CK+PULSESIM"))
        {
            vbb_OUTPUT_Print("INFO: Got command \"");
            vbb_OUTPUT_PrintDebug(MessageBody);
            vbb_OUTPUT_Print("\"\r\n");
            
            sscanf(MessageBody + vbb_GetLen("CK+PULSESIM,"), "%d", &_utils_pulse_multiply);            
            
            if (_utils_pulse_multiply == 0)
            {
                //vbb_PIN_Init(&pin_pulse, GPIOD, 7, PIN_MODE_FLOATING_INPUT);
                _utils_pulse_simulate = FALSE;
                _utils_pulse_multiply = 1;
            }
            else
            {
                //vbb_PIN_Init(&pin_pulse, GPIOD, 7, PIN_MODE_GENERAL_OUTPUT_PUSH_PULL);
                _utils_pulse_simulate = TRUE;
            }            
            
            if (_utils_pulse_simulate)
            {
                if (Number)
                    vbb_GSM_SendSMS(Number, "Pulse simulation mode enabled.");
                else
                    vbb_OUTPUT_Print("INFO: Pulse simulation mode enabled.\r\n");
            }
            else
            {
                if (Number)
                    vbb_GSM_SendSMS(Number, "Pulse simulation mode disable.");
                else
                    vbb_OUTPUT_Print("INFO: Pulse simulation mode disable.\r\n");
            }            
            return TRUE;
        }   
        else if (vbb_StartWith(MessageBody, "CK+GSMREBOOT"))
        {
            GSM_State = GSM_BEFORE_REBOOT;
            return TRUE;
        }  
    }

    vbb_OUTPUT_Print("INFO: Unknown command: \"");
    vbb_OUTPUT_PrintDebug(MessageBody);
    vbb_OUTPUT_Print("\"\r\n");
    return FALSE;
}

unsigned int vbb_INFLASH_Write(unsigned int Addr, const char * Data, int DataLen)
{
    unsigned short value = 0;    
    unsigned int addr = Addr;
    unsigned int count = 0;  
    char l = 0;
    char h = 0;
    unsigned int rt;
    
    if (addr % 2 != 0)
    {
        addr--;    
        value = (*((unsigned char *)addr)) & 0x00FF;
        value |= ((Data[0] << 8) & 0xFF00);
        rt = vbb_INFLASH_Program(addr, value);
        if (rt == 0)
            return 0;
        vbb_OUTPUT_WriteByte(Data[0]);
        Data++;
        DataLen--;
        addr += 2;
    }
    
    while (DataLen > 0)
    {
        count = 0;
        l = Data[0];
        value = Data[0] & 0x00FF;
        Data++;
        DataLen--;
        count++;
        if (DataLen > 0)
        {
            h = Data[0];
            value |= ((Data[0] << 8) & 0xFF00);
            Data++;
            DataLen--;
            count++;
        }
        rt = vbb_INFLASH_Program(addr, value);
        if (rt == 0)
            return 0;
        vbb_OUTPUT_WriteByte(l);
        vbb_OUTPUT_WriteByte(h);
        addr += count;
    } 
    
    return addr;
}

#endif
